#ifndef __COMMON_COORD_TRANS_H__
#define __COMMON_COORD_TRANS_H__

#include <vector>
#include <cmath>
#include <algorithm>

namespace global_path_planner {
namespace common{
namespace coordtrans {
// /*
//     生成探索矩阵
// */
// static std::vector<std::vector<float>> GenExploreList(int ksize){
//   	std::vector<std::vector<float>> explore_lists;
//   	std::vector<float> list;
//   	for(int i = 0; i < 2 * ksize + 1; i++){
//   	    for(int j = 0; j < 2 * ksize + 1; j++){
//   	        list.clear();
//   	        int di = i - ksize;
//   	        int dj = j - ksize;
//   	        if(di == 0 && dj == 0) continue;
//   	        list.push_back(di);
//   	        list.push_back(dj);
//   	        list.push_back(std::sqrt(di * di + dj * dj));
//   	        explore_lists.push_back(list);
//   	    }
//   	}
//   	std::sort(explore_lists.begin(), explore_lists.end(),
//   	            [](std::vector<float>& l,
//   	              std::vector<float>& r) -> bool
//   	            {
//   	              return l[2] < r[2];
//   	            });
//   	return explore_lists;
// }

/*
    真实坐标转换为地图栅格坐标
    @resolution: 地图分辨率
    @bias:  地图左下角对应的实际空间坐标(OccupancyGrid的Origion属性)
*/
static std::pair<int, int> Realxy2Pixelxy(std::pair<float, float>& pixel, 
                                        float resolution, float bias_x, 
                                        float bias_y){
    int __x = (int)std::round((pixel.first - bias_x) / resolution);
    int __y = (int)std::round((pixel.second - bias_y) / resolution);
    return std::make_pair(__x, __y);
}

/*
    栅格转化为实际坐标
    @resolution: 地图分辨率
    @bias:  地图左下角对应的实际空间坐标(OccupancyGrid的Origion属性)
*/
static std::pair<float, float> Pixelxy2Realxy(std::pair<float, float>& real, 
                                            float resolution, float bias_x, 
                                            float bias_y){
    float __x = real.first * resolution + bias_x;
    float __y = real.second * resolution + bias_y;
    return std::make_pair(__x, __y);
}
} // coordtrans
} // common
} // global_path_planner

#endif // __COMMON_COORD_TRANS_H__